#include <hal/ahci/sata.h>
#include <hal/ahci/ahci.h>

#include <lib/math.h>

#include <mm/valloc.h>

#define LOG_TAG "SATA"
#include <log.h>

void sata_create_fis(sata_cmd_fis_t* cmd_fis, u8 command, u64 lba, u16 sector_count) {
    cmd_fis->head.type = SATA_REG_FIS_H2D;
    cmd_fis->head.options = SATA_REG_FIS_COMMAND;
    cmd_fis->head.status_cmd = command;
    cmd_fis->dev = 0;

    cmd_fis->lba0 = SATA_LBA_COMPONENT(lba, 0);
    cmd_fis->lba8 = SATA_LBA_COMPONENT(lba, 8);
    cmd_fis->lba16 = SATA_LBA_COMPONENT(lba, 16);
    cmd_fis->lba24 = SATA_LBA_COMPONENT(lba, 24);

    cmd_fis->lba32 = SATA_LBA_COMPONENT(lba, 32);
    cmd_fis->lba40 = SATA_LBA_COMPONENT(lba, 40);

    cmd_fis->count = sector_count;
}

void sata_submit(hba_dev_t* dev, blkio_req_t* req) {
    hba_port_t* port = dev->port;
    hba_cmdh_t* header;
    hba_cmdt_t* table;

    int write = !!(req->flags & BLKIO_WRITE);
    int slot = hba_prepare_cmd(port, &table, &header);
    hba_bind_parted_buf(header, table, req->pb);

    header->options |= HBA_CMDH_WRITE * write;

    size_t count = ICEIL(pbuffer_size(req->pb), port->device->block_size);
    sata_cmd_fis_t* fis = (sata_cmd_fis_t*)table->command_fis;

    if (port->device->flags & HBA_DEV_FEXTLBA) {
        sata_create_fis(fis, write ? ATA_WRITE_DMA_EXT : ATA_READ_DMA_EXT, req->blk_addr, count);
    } else {
        sata_create_fis(fis, write ? ATA_WRITE_DMA : ATA_READ_DMA, req->blk_addr, count);
    }
    // make sure this is LBA mode
    fis->dev = (1 << 6);

    hba_cmds_t* cmds = valloc(sizeof(hba_cmds_t));
    *cmds = (hba_cmds_t){.cmd_table = table, .state_ctx = req};
    ahci_post(port, cmds, slot);
}

void sata_read_error(hba_port_t* port) {
    u32 tfd = port->regs[HBA_REG_PxTFD];
    port->device->last_result.error = (tfd >> 8) & 0xFF;
    port->device->last_result.status = tfd & 0xFF;
}